
#include "planner_pkg/fixed.hpp"
#include "queue"
#include "unordered_map"

namespace planner_pkg
{

    /**
     * @brief  Constructs the planner
     * @param nx The x size of the map
     * @param ny The y size of the map
     */
    Fixed::Fixed(int nx, int ny) :logger_(rclcpp::get_logger("Fixed"))
    {
        setMapSize(nx, ny);
        waypoint_neighbors_ = agv_util::load_waypoint_neighbors();
    }
    // 析构函数
    Fixed::~Fixed()
    {
        if (costmap_)
        {
            delete[] costmap_;
        }
    }

    void Fixed::setMapSize(int nx_size, int ny_size) // 223,163
    {
        this->nx = nx_size;
        this->ny = ny_size;
        this->ns = nx * ny;
        // RCLCPP_INFO(logger_, "map size: %d, %d", nx, ny);
        costmap_ = new unsigned char[ns];
    }

    void Fixed::setCostmap(unsigned char *cmap)
    {
        // 一维数组，索引 int index = y * width + x;
        costmap_ = cmap;
    }

    void
    Fixed::setGoal(int *g)
    {
        goal_[0] = g[0];
        goal_[1] = g[1];
        RCLCPP_DEBUG(logger_, "goal: %d,%d", goal_[0], goal_[1]);
        RCLCPP_DEBUG(logger_, "goal cost: %d", costmap_[g[0] + g[1] * nx]);
    }

    void
    Fixed::setStart(int *g)
    {
        start_[0] = g[0];
        start_[1] = g[1];
        // RCLCPP_DEBUG(logger_, "start: %d,%d", start_[0], start_[1]);
    }

    //
    std::shared_ptr<std::vector<std::shared_ptr<PointNodeFixed>>> Fixed::searchPath()
    {
        if ( point_node_list_ == nullptr ||point_node_list_->empty())
        {
            RCLCPP_ERROR(logger_, "point_node_list_ is empty");
            return nullptr;
        }
        std::priority_queue<std::shared_ptr<PointNodeFixed>, std::vector<std::shared_ptr<PointNodeFixed>>, CompareNode> openList; // 创建优先队列
        std::unordered_map<int, std::shared_ptr<PointNodeFixed>> openListMap;
        std::unordered_map<int, std::shared_ptr<PointNodeFixed>> closedListMap;

        auto startNode = std::make_shared<PointNodeFixed>(start_[0], start_[1], nullptr, 0, 0, "start");

        openList.push(startNode); // 将起始节点加入待评估集合
        openListMap[start_[0] * ny + start_[1]] = startNode;

        while (!openList.empty())
        {
            auto currentNode = openList.top();
            openList.pop();
            openListMap.erase(currentNode->x * ny + currentNode->y); // 移除节点

            // 到达目标点，则返回路径
            if (reach_goal(currentNode->x, currentNode->y))
            {
                return getPath(currentNode);
            }
            // 遍历当前节点的相邻节点
            closedListMap[currentNode->x * ny + currentNode->y] = currentNode; // 将当前节点加入已评估集合

            auto neighbor_nodes = get_neighbor_nodes_by_xy(currentNode->x, currentNode->y);
            if (neighbor_nodes->empty())
            {
                RCLCPP_ERROR(logger_, "neighbor_nodes is empty");
                return {};
            }
            for (auto neighbor_node : *neighbor_nodes)
            {

                // 超出坐标系
                if (neighbor_node->x >= nx || neighbor_node->y >= ny || neighbor_node->x < 0 || neighbor_node->y < 0)
                {
                    continue;
                }
                if (costObs(neighbor_node->x, neighbor_node->y) >= COST_OBS_ROS) // 障碍物或在膨胀半径内
                {
                    continue;
                }

                int neighborKey = neighbor_node->x * ny + neighbor_node->y;
                if (closedListMap.find(neighborKey) != closedListMap.end())
                {
                    continue; // 跳过已经在closedList中的节点
                }
                int gCost = currentNode->g + std::hypot(currentNode->x - neighbor_node->x, currentNode->y - neighbor_node->y);
                int hCost = heuristic(neighbor_node->x, neighbor_node->y);
                std::shared_ptr<PointNodeFixed> neighborNode = openListMap[neighborKey];
                // 如果邻居节点不在待评估集合中，则将其加入
                if (neighborNode == nullptr)
                {
                    neighborNode = std::make_shared<PointNodeFixed>(neighbor_node->x,
                                                               neighbor_node->y,
                                                               currentNode,
                                                               gCost,
                                                               hCost,
                                                               neighbor_node->name);
                    openList.push(neighborNode);
                    openListMap[neighborKey] = neighborNode;
                }
                // 如果邻居节点已经在待评估集合中，检查是否找到了更优路径
                else if (gCost < neighborNode->g)
                {
                    neighborNode->g = gCost;
                    neighborNode->f = neighborNode->g + neighborNode->h;
                    neighborNode->parent = currentNode;
                }
            }
        }

        return {}; // Return an empty path if no path is found
    }

    // 判断是否到达目标点，目标点的邻居节点
    bool Fixed::reach_goal(int x, int y)
    {
        auto neighborPoints = get_neighbor_nodes_by_xy(goal_[0], goal_[1]);
        for (auto neighborPoint : *neighborPoints)
        {
            if (neighborPoint->x == x && neighborPoint->y == y)
            {
                return true;
            }
        }
        return false;
    }

    // 重构路径函数，返回从起始节点到目标节点的路径
    std::shared_ptr<std::vector<std::shared_ptr<PointNodeFixed>>> Fixed::getPath(std::shared_ptr<PointNodeFixed> current)
    {
        auto path = std::make_shared<std::vector<std::shared_ptr<PointNodeFixed>>>();
        while (current != nullptr)
        {
            path->push_back(current);
            current = current->parent;
        }
        // 路径倒置
        std::reverse(path->begin(), path->end());
        auto goal_node = std::make_shared<PointNodeFixed>(goal_[0], goal_[1], nullptr, 0, 0, "goal");
        path->push_back(goal_node);
        return path;
    }

    int Fixed::heuristic(int nodeX, int nodeY)
    {
        // 欧几里得距离
        double dist = std::hypot(nodeX - goal_[0], nodeY - goal_[1]);
        return dist;
    }

    int Fixed::costObs(int x, int y)
    {
        int weight = 2;
        for (int dx = -weight; dx < weight; dx++)
        {
            for (int dy = -weight; dy < weight; dy++)
            {
                int node_x = x + dx;
                int node_y = y + dy;
                // 超出坐标系
                if (node_x >= nx || node_y >= ny || node_x < 0 || node_y < 0)
                {
                    continue;
                }
                if (costmap_[node_y * nx + node_x] >= COST_OBS_ROS) // 障碍物或在膨胀半径内
                {
                    return costmap_[node_y * nx + node_x];
                }
            }
        }
        return 0;
    }

    void Fixed::set_node_list(std::shared_ptr<std::vector<std::shared_ptr<PointNodeFixed>>> list)
    {
        point_node_list_ = list;
        RCLCPP_INFO(logger_,"set_node_list");
    }
    std::shared_ptr<PointNodeFixed> Fixed::get_node_of_name(std::string name)
    {
        for (auto &point_node : *point_node_list_)
        {
            if (point_node->name == name)
            {
                return point_node;
            }
        }
        return {};
    }
    // 根据坐标xy获取当前的相邻点
    std::shared_ptr<std::vector<std::shared_ptr<PointNodeFixed>>> Fixed::get_neighbor_nodes_by_xy(int x, int y)
    {
        auto near_points = std::make_shared<std::vector<std::shared_ptr<PointNodeFixed>>>();
        if (point_node_list_->empty())
        {
            return near_points;
        }
        double s1_min = 999;
        for (auto &point_node : *point_node_list_)
        {
            auto neighbor_list = get_neighbor_nodes_by_name(point_node->name);
            // 在点位上
            if (point_node->x == x && point_node->y == y)
            {
                return neighbor_list;
            }
            double s1 = std::hypot(point_node->x - x, point_node->y - y);
            if (s1 < s1_min)
            {
                s1_min = s1;
                near_points->clear();
                near_points->push_back(point_node);
            }
            // 不在点位上
            for (auto &neighbor_node : *neighbor_list)
            { // 遍历相邻点
                // x,y 到 point_node的距离  s1
                // x,y 到 neighbor_node的距离 s2
                // point_node 到 neighbor_node的距离 dist
                // s1<dist && s2<dist
                double s2 = std::hypot(neighbor_node->x - x, neighbor_node->y - y);
                double dist = std::hypot(point_node->x - neighbor_node->x, point_node->y - neighbor_node->y);
                if (s1 < dist && s2 < dist)
                {
                    near_points->clear();
                    near_points->push_back(point_node);
                    near_points->push_back(neighbor_node);
                    return near_points;
                }
            }
        }
        return near_points;
    }
    // 根据name获取邻居点
    std::shared_ptr<std::vector<std::shared_ptr<PointNodeFixed>>> Fixed::get_neighbor_nodes_by_name(std::string name)
    {
        auto neighbor_list = std::make_shared<std::vector<std::shared_ptr<PointNodeFixed>>>();
        for (auto &waypointNeighbor : waypoint_neighbors_)
        {
            if (waypointNeighbor.name == name)
            {
                for (auto &neighborName : waypointNeighbor.neighbors)
                {
                    neighbor_list->push_back(get_node_of_name(neighborName));
                }
                break;
            }
        }
        
        return neighbor_list;
    }
} // namespace planner_pkg